#include "platform_controller_server/launch.h"


Launch::Launch(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private)
	: nh_(nh)
	, nh_private_(nh_private)
	, map_save_path_("")
	, launch_pid_path_("")
	, shell_path_("")
	, rosbag_name_("")
{
	nh_private_.param("map_save_path",map_save_path_,map_save_path_);
	nh_private_.param("launch_pid_path",launch_pid_path_,launch_pid_path_);
	nh_private_.param("shell_path",shell_path_,shell_path_);

	ROS_INFO("Map save path: %s",map_save_path_.c_str());
	ROS_INFO("Pid save path: %s",launch_pid_path_.c_str());
	ROS_INFO("Shell save path: %s",shell_path_.c_str());

	memset(buffer,0,512);

	//TTS notification
	pub_tts            = nh_.advertise<std_msgs::String>("/tts",1);

	//mapping
	sub_launchGmapping = nh_.subscribe<std_msgs::Empty>("launch_gmapping",1,&Launch::onLaunchGamppingCallback,this);
	sub_closeGmapping  = nh_.subscribe<std_msgs::Empty>("close_gmapping",1,&Launch::onCloseGmappingCallback,this);
	sub_saveMap        = nh_.subscribe<std_msgs::String>("save_map",1,&Launch::onSaveMapCallback,this);

	//AMCL
	sub_launcAMCL      = nh_.subscribe<std_msgs::String>("launch_amcl",1,&Launch::onLaunchAMCLcallback,this);
	sub_closeAMCL      = nh_.subscribe<std_msgs::Empty>("close_amcl",1,&Launch::onCloseAMCLcallback,this);

	//ROSBAG
	sub_launchROSBagRecord = nh_.subscribe<std_msgs::String>("launch_rosbag_record",1,&Launch::onLaunchROSBagRecordCallback,this);
	sub_closeROSBagRecord  = nh_.subscribe<std_msgs::Empty>("close_rosbag_record",1,&Launch::onCloseROSBagRecordCallback,this);


	timer_test             = nh_.subscribe<std_msgs::Empty>("timer_test",1,&Launch::onTimerTest,this);
	download_timer_        = nh_.createTimer(ros::Duration(1), &Launch::onDownloadTimerCallback,this,true);
	upload_timer_          = nh_.createTimer(ros::Duration(3), &Launch::onUploadTimerCallback,this,true);
	download_timer_.stop();
	upload_timer_.stop();
}
Launch::~Launch(){}

//MAPPING
void Launch::onLaunchGamppingCallback(const std_msgs::Empty::ConstPtr& msg)
{
	ROS_INFO("Launch gmapping");
	std_msgs::String tts_msg;
	tts_msg.data = "正在启动制图，请稍后...";
	pub_tts.publish(tts_msg);
	ros::Duration(3).sleep();

	pid_t generateMapPid = fork();
    if (generateMapPid < 0)
		ROS_INFO("fork error");
   	else if(generateMapPid == 0)
   	{	
		long pid_generateMap = getpid();
		ROS_INFO("in child process pid: %ld",pid_generateMap);
		sprintf(buffer,"%s/launch_gmapping.sh",shell_path_.c_str());
		if(execl("/bin/sh","sh",buffer,NULL)<0)
		{
			perror("Error on execl");
			return;
		}
		tts_msg.data = "制图启动完成，正在制图中...";
		pub_tts.publish(tts_msg);
    }

}

void Launch::onCloseGmappingCallback(const std_msgs::Empty::ConstPtr& msg)
{
	ROS_INFO("Kill gmapping");
	memset(buffer,0,512);
	sprintf(buffer,"%s/gmapping.txt",launch_pid_path_.c_str());
	ifstream launch_pid_file(buffer,ios::in);

	if(!launch_pid_file)
	{
		ROS_WARN("open launch pid file failed!");
		return;		
	}

	char buffer[10];
	if(!launch_pid_file.eof())
	{	
		launch_pid_file.getline(buffer,10);
		pid_t pid = atoi(buffer);
		ROS_INFO("%d",pid);
		int result = kill(pid,SIGINT);
		if(result<0)
		{	
			ROS_ERROR("send SIGINT signal failed! %s",strerror(errno));
		}
	}
}

void Launch::onSaveMapCallback(const std_msgs::String::ConstPtr& msg)
{
	ROS_INFO("Save Map");
	pid_t generateMapPid;
    generateMapPid = fork();
    if (generateMapPid < 0)
	{
		ROS_INFO("fork error");
	}
   	else if(generateMapPid == 0)
   	{	
		long pid_generateMap = getpid();
		ROS_INFO("in child process pid: %ld",pid_generateMap);
		
		memset(buffer,0,512);
		sprintf(buffer,"%s/launch_savemap.sh",shell_path_.c_str());
		if(execl("/bin/sh","sh",buffer,msg->data.c_str(),NULL)<0)
		{	
			perror("Error on execl");
		}
    }
}


//AMCL
void Launch::onLaunchAMCLcallback(const std_msgs::String::ConstPtr& msg)
{
	ROS_INFO("Launch AMCL");
	std_msgs::String tts_msg;
	tts_msg.data = "正在启动导航，请稍后...";
	pub_tts.publish(tts_msg);

	//解压数据包
	download_timer_.start();

	ros::Duration(3).sleep();


	pid_t generateMapPid = fork();
    if (generateMapPid < 0)
		ROS_INFO("fork error");
   	else if(generateMapPid == 0)
   	{	
		long pid_generateMap = getpid();
		ROS_INFO("in child process pid: %ld",pid_generateMap);
		memset(buffer,0,512);
		sprintf(buffer,"%s/launch_amcl.sh",shell_path_.c_str());

		if(execl("/bin/sh","sh",buffer,msg->data.c_str(),NULL)<0)
		{
			perror("Error on execl");
			return;
		}

		tts_msg.data = "导航启动成功，可以开始送餐了...";
		pub_tts.publish(tts_msg);
    }
}


void Launch::onCloseAMCLcallback(const std_msgs::Empty::ConstPtr& msg)
{
	ROS_INFO("Kill AMCL");
	std_msgs::String tts_msg;
	tts_msg.data = "正在关闭导航，请稍后...";
	pub_tts.publish(tts_msg);
	ros::Duration(3).sleep();

	memset(buffer,0,512);
	sprintf(buffer,"%s/amcl.txt",launch_pid_path_.c_str());
	ifstream launch_pid_file(buffer,ios::in);

	if(!launch_pid_file)
	{
		ROS_WARN("open launch pid file failed!");
		return;		
	}

	char buffer[10];
	if(!launch_pid_file.eof())
	{	
		launch_pid_file.getline(buffer,10);
		pid_t pid = atoi(buffer);
		ROS_INFO("%d",pid);
		int result = kill(pid,SIGINT);
		if(result<0)
		{	
			ROS_ERROR("send SIGINT signal failed! %s",strerror(errno));
		}

		tts_msg.data = "导航服务已结束...";
		pub_tts.publish(tts_msg);
	}
}

//ROSBAG
void Launch::onLaunchROSBagRecordCallback(const std_msgs::String::ConstPtr& msg)
{
	rosbag_name_ = msg->data;

	ROS_INFO("Launch ROS bag record");
	std_msgs::String tts_msg;
	tts_msg.data = "正在打开数据包录制功能，请稍后...";
	pub_tts.publish(tts_msg);
	ros::Duration(3).sleep();

	pid_t generateMapPid = fork();
    if (generateMapPid < 0)
		ROS_INFO("fork error");
   	else if(generateMapPid == 0)
   	{	
		long pid_generateMap = getpid();
		ROS_INFO("in child process pid: %ld",pid_generateMap);
		memset(buffer,0,512);
		sprintf(buffer,"%s/launch_rosbag_record.sh",shell_path_.c_str());
		if(execl("/bin/sh","sh",buffer,msg->data.c_str(),NULL)<0)
		{
			perror("Error on execl");
			return;
		}

		tts_msg.data = "开始录制数据包了...";
		pub_tts.publish(tts_msg);
    }
}


void Launch::onCloseROSBagRecordCallback(const std_msgs::Empty::ConstPtr& msg)
{
	ROS_INFO("Kill ROSBAG record");
	std_msgs::String tts_msg;
	tts_msg.data = "正在关闭数据包录制功能，请稍后...";
	pub_tts.publish(tts_msg);
	ros::Duration(3).sleep();

	memset(buffer,0,512);
	sprintf(buffer,"%s/rosbag.txt",launch_pid_path_.c_str());
	ifstream launch_pid_file(buffer,ios::in);

	if(!launch_pid_file)
	{
		ROS_WARN("open launch pid file failed!");
		return;		
	}

	char buffer[10];
	if(!launch_pid_file.eof())
	{	
		launch_pid_file.getline(buffer,10);
		pid_t pid = atoi(buffer);
		ROS_INFO("%d",pid);
		int result = kill(pid,SIGINT);
		if(result<0)
		{	
			ROS_ERROR("send SIGINT signal failed! %s",strerror(errno));
		}
		tts_msg.data = "数据包录制已关闭";
		pub_tts.publish(tts_msg);
	}

	//TODO:将数据包与导航文件压缩并拷贝到指定的目录中
	upload_timer_.start();
}


void Launch::onTimerTest(const std_msgs::Empty::ConstPtr& msg)
{
	static bool flag = true;
	if(flag)
	{
		download_timer_.start();
		upload_timer_.start();
	}
}


void Launch::onUploadTimerCallback(const ros::TimerEvent& e)
{
	ROS_INFO("Upload rosbag and navigation file");

	//将数据包与导航文件压缩并拷贝到指定的目录中
	std_msgs::String tts_msg;
	tts_msg.data = "正在打包文件，请稍后...";
	pub_tts.publish(tts_msg);
	ros::Duration(3).sleep();

	pid_t generateMapPid = fork();
    if (generateMapPid < 0)
		ROS_INFO("fork error");
   	else if(generateMapPid == 0)
   	{	
		long pid_generateMap = getpid();
		ROS_INFO("in child process pid: %ld",pid_generateMap);
		memset(buffer,0,512);
		sprintf(buffer,"%s/upload_data.sh",shell_path_.c_str());

		if(execl("/bin/sh","sh",buffer,rosbag_name_.c_str(),NULL)<0)
		{
			perror("Error on execl");
			return;
		}

		tts_msg.data = "导航启动成功，可以开始送餐了...";
		pub_tts.publish(tts_msg);
    }

	upload_timer_.stop();
}



void Launch::onDownloadTimerCallback(const ros::TimerEvent& e)
{
	ROS_INFO("Down map and navigation file");

	pid_t generateMapPid = fork();
    if (generateMapPid < 0)
		ROS_INFO("fork error");
   	else if(generateMapPid == 0)
   	{	
		long pid_generateMap = getpid();
		ROS_INFO("in child process pid: %ld",pid_generateMap);
		memset(buffer,0,512);
		sprintf(buffer,"%s/download_data.sh",shell_path_.c_str());

		if(execl("/bin/sh","sh",buffer,NULL)<0)
		{
			perror("Error on execl");
			return;
		}
    }

	download_timer_.stop();
}






int main(int argc, char**argv)
{
	ros::init(argc,argv,"launch_node");
	ros::NodeHandle nh,nh_private("~");
	Launch launch_node(nh,nh_private);
	ros::spin();
	return 0;
}
